// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
//
// This is free software; you can redistribute it and/or modify it under
// the terms of the GNU Lesser General Public License as published by the
// Free Software Foundation; either version 2.1 of the License, or (at
// your option) any later version.
//

/// @file		Common.cpp
/// @brief		Common utility routines for the ArduPilot libraries.
///
/// @note		Exercise restraint adding code here; everything in this
///				library will be linked with any sketch using it.
///

#include "Common.h"
#include "WProgram.h"

long last50Hz;
long last20Hz;
long last10Hz;
long last5Hz;
long last2Hz;
long last1Hz;
long last10Sec;

// Constructors ////////////////////////////////////////////////////////////////

LoopTimer::LoopTimer()
{
}

void LoopTimer::Init(void) {
	last1Hz = last2Hz = last5Hz = last10Hz = last20Hz = last50Hz = millis();
}

uint8_t LoopTimer::Timer50Hz(void) {
	if (millis() - last50Hz > 19) {
		last50Hz = millis();
		return TRUE;
	}
	return FALSE;
}

uint8_t LoopTimer::Timer20Hz(void) {
	if (millis() - last20Hz > 49) {
		last20Hz = millis();
		return TRUE;
	}
	return FALSE;
}

uint8_t LoopTimer::Timer10Hz(void) {
	if (millis() - last10Hz > 99) {
		last10Hz = millis();
		return TRUE;
	}
	return FALSE;
}

uint8_t LoopTimer::Timer5Hz(void) {
	if (millis() - last5Hz > 199) {
		last5Hz = millis();
		return TRUE;
	}
	return FALSE;
}

uint8_t LoopTimer::Timer2Hz(void) {
	if (millis() - last2Hz > 499) {
		last2Hz = millis();
		return TRUE;
	}
	return FALSE;
}

uint8_t LoopTimer::Timer1Hz(void) {
	if (millis() - last1Hz > 999) {
		last1Hz = millis();
		return TRUE;
	}
	return FALSE;
}

uint8_t LoopTimer::Timer10Sec(void) {
	if (millis() - last10Sec > 9999) {
		last10Sec = millis();
		return TRUE;
	}
	return FALSE;
}

void WaitFor(long wf) {
	long endTime = (wf + millis());
	while (millis() < endTime) {}
}

